/*
 * DistortionMoveThoughtDecorator.cpp
 *
 *  Created on: 09-01-2011
 *      Author: Piotr
 */

#include "AI/thoughts/DistortionMoveThoughtDecorator.hpp"
#include "utils/MathUtils.hpp"
#include "utils/Randomizer.hpp"

DistortionMoveThoughtDecorator::DistortionMoveThoughtDecorator(MoveThought *moveThought,
		float angleDistortionPercent) :
	moveThought(boost::shared_ptr<MoveThought>(moveThought)), angleDistortionPercent(
			MathUtils::trimValue(angleDistortionPercent, 0.0f, 100.0f))
{

}

void DistortionMoveThoughtDecorator::execute(Individual* individual)
{
	Vector distortion = getDistortionVector(moveThought.get()->getDirection(), individual);
	MoveThought(moveThought.get()->getDirection() + distortion, moveThought.get()->getLength()).execute(
			individual);
}

Vector DistortionMoveThoughtDecorator::getDistortionVector(const Vector& direction,
		Individual* individual)
{
	const float maxAngleChange = MathUtils::toRadians(
			individual->getGenotype().getMaxAngularVelocityInDegreeses());
	const float angleChange = getDistortionChange(angleDistortionPercent, maxAngleChange);
	const float angle = direction.getAngleInRadians();

	return Vector(cosf(angle + angleChange), sinf(angle + angleChange));
}

float DistortionMoveThoughtDecorator::getDistortionChange(const int maxPercentRange,
		const float maxChange) const
{
	const int randomPercent = ((Randomizer::nextInt(maxPercentRange)) - maxPercentRange / 2);
	const float percentFraction = randomPercent / 100.0f;
	return maxChange * percentFraction;
}
